#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np


class CameraPublisher(Node):
    def __init__(self):
        super().__init__('camera_publisher')
        
        # 声明参数
        self.declare_parameter('camera_index', 0)
        self.declare_parameter('image_width', 640)
        self.declare_parameter('image_height', 480)
        self.declare_parameter('fps', 30)
        self.declare_parameter('topic_name', '/camera/color/image_raw')
        self.declare_parameter('frame_id', 'camera_frame')
        
        # 获取参数值
        self.camera_index = self.get_parameter('camera_index').get_parameter_value().integer_value
        self.image_width = self.get_parameter('image_width').get_parameter_value().integer_value
        self.image_height = self.get_parameter('image_height').get_parameter_value().integer_value
        self.fps = self.get_parameter('fps').get_parameter_value().integer_value
        self.topic_name = self.get_parameter('topic_name').get_parameter_value().string_value
        self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
        
        # 初始化CV桥接器
        self.bridge = CvBridge()
        
        # 创建发布者
        self.publisher = self.create_publisher(Image, self.topic_name, 10)
        
        # 初始化摄像头
        self.cap = cv2.VideoCapture(self.camera_index)
        
        if not self.cap.isOpened():
            self.get_logger().error(f'无法打开摄像头 {self.camera_index}')
            return
        
        # 设置摄像头参数
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.image_width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.image_height)
        self.cap.set(cv2.CAP_PROP_FPS, self.fps)
        
        # 验证设置的参数
        actual_width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
        actual_height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
        actual_fps = int(self.cap.get(cv2.CAP_PROP_FPS))
        
        self.get_logger().info(f'摄像头初始化成功')
        self.get_logger().info(f'摄像头索引: {self.camera_index}')
        self.get_logger().info(f'设置分辨率: {self.image_width}x{self.image_height}, 实际分辨率: {actual_width}x{actual_height}')
        self.get_logger().info(f'设置FPS: {self.fps}, 实际FPS: {actual_fps}')
        self.get_logger().info(f'发布话题: {self.topic_name}')
        self.get_logger().info(f'坐标系ID: {self.frame_id}')
        
        # 创建定时器，根据FPS计算发布频率
        timer_period = 1.0 / self.fps
        self.timer = self.create_timer(timer_period, self.timer_callback)
        
    def timer_callback(self):
        """定时器回调函数，获取并发布图像"""
        ret, frame = self.cap.read()
        
        if not ret:
            self.get_logger().warn('无法从摄像头读取图像')
            return
        
        try:
            # 将OpenCV图像转换为ROS Image消息
            ros_image = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
            
            # 设置时间戳和坐标系ID
            ros_image.header.stamp = self.get_clock().now().to_msg()
            ros_image.header.frame_id = self.frame_id
            
            # 发布图像
            self.publisher.publish(ros_image)
            
        except Exception as e:
            self.get_logger().error(f'发布图像时出错: {str(e)}')
    
    def destroy_node(self):
        """节点销毁时释放摄像头资源"""
        if hasattr(self, 'cap') and self.cap.isOpened():
            self.cap.release()
            self.get_logger().info('摄像头资源已释放')
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    
    camera_publisher = CameraPublisher()
    
    try:
        rclpy.spin(camera_publisher)
    except KeyboardInterrupt:
        pass
    finally:
        camera_publisher.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()